In [1]:

    

import nbimport
from Aircraft_Kinematics import body_b, body_w, frame_i, frame_b, frame_w, bke, t, m, point_cm_w, \
    V_T, alpha, beta, sol_rot, sol_euler_rates, sol_vel_wind, P, Q, R, phi, theta, psi, M_bx, M_by, M_bz, \
    point_cm_b, M_wx, M_wy, M_wz, J_x, J_y, J_z, J_xz, sol_trans_wind, F_wx, F_wy, F_wz
import sympy
import scipy.optimize
import statespace
import pylab as pl
import control
g, L, C, D, T = sympy.symbols('g, L, C, D, T')
ail, elv, rdr, thr = sympy.symbols('ail, elv, rdr, thr')
%load_ext autoreload
%autoreload 2
%precision 3


    

    




importing IPython notebook from Aircraft Kinematics.ipynb






    
Out[1]:





u'%.3f'

In [2]:

    

F = m*g*frame_i.z - L(t)*frame_w.z - C(t)*frame_w.y - D(t)*frame_w.x + T(t)*frame_b.x
F


    

    
Out[2]:





$$g m\mathbf{\hat{i}_z} -  D\mathbf{\hat{w}_x} -  C\mathbf{\hat{w}_y} -  L\mathbf{\hat{w}_z} + T\mathbf{\hat{b}_x}$$

In [3]:

    

di_L_i = bke(body_w.linear_momentum(frame_i), frame_i, frame_w, t) 
sol_trans = sympy.solve((di_L_i - F).to_matrix(frame_w), [xi(t).diff(t) for xi in [V_T, alpha, beta]])
sol_trans


    

    
Out[3]:





$$\left \{ \dot{V}_{T} : g \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\theta\right) + g \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\theta\right) - g \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) - \frac{D}{m} + \frac{T \operatorname{cos}\left(\alpha\right)}{m} \operatorname{cos}\left(\beta\right), \quad \dot{\alpha} : \frac{1}{m V_{T}} \left(- g m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\theta\right) + g m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\beta\right) + g m \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\theta\right) - m P V_{T} \operatorname{sin}\left(\beta\right) + m Q V_{T} \operatorname{cos}\left(\beta\right) - L - T \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right), \quad \dot{\beta} : \frac{1}{m V_{T} \operatorname{cos}\left(\alpha\right)} \left(g m \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\theta\right) + g m \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\beta\right) \operatorname{cos}\left(\theta\right) + m P V_{T} \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right) + m Q V_{T} \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right) - m R V_{T} \operatorname{cos}\left(\alpha\right) - C - T \operatorname{sin}\left(\beta\right)\right)\right \}$$

In [4]:

    

M = frame_w.x*M_wx(t) + frame_w.y*M_wy(t) + frame_w.z*M_wz(t)


    

In [5]:

    

di_H_i = bke(body_b.angular_momentum(point_cm_b, frame_i), frame_i, frame_b, t)
di_H_i


    

    
Out[5]:





$$(J_{x} \dot{P} + J_{xz} \dot{R} - J_{y} Q R + \left(J_{xz} P + J_{z} R\right) Q)\mathbf{\hat{b}_x} + (J_{y} \dot{Q} + \left(J_{x} P + J_{xz} R\right) R - \left(J_{xz} P + J_{z} R\right) P)\mathbf{\hat{b}_y} + (J_{xz} \dot{P} + J_{y} P Q + J_{z} \dot{R} - \left(J_{x} P + J_{xz} R\right) Q)\mathbf{\hat{b}_z}$$

In [6]:

    

sol_rot = sympy.solve((M - di_H_i).to_matrix(frame_b), [xi(t).diff(t) for xi in [P, Q, R]])
sol_rot


    

    
Out[6]:





$$\left \{ \dot{P} : - \frac{1}{J_{x} J_{z} - J_{xz}^{2}} \left(J_{xz} \left(J_{x} P Q + J_{xz} Q R - J_{y} P Q + M_{wx} \operatorname{sin}\left(\alpha\right) + M_{wz} \operatorname{cos}\left(\alpha\right)\right) + J_{z} \left(J_{xz} P Q - J_{y} Q R + J_{z} Q R - M_{wx} \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) + M_{wy} \operatorname{sin}\left(\beta\right) + M_{wz} \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\right), \quad \dot{Q} : \frac{1}{J_{y}} \left(- J_{x} P R + J_{xz} P^{2} - J_{xz} R^{2} + J_{z} P R + M_{wx} \operatorname{sin}\left(\beta\right) \operatorname{cos}\left(\alpha\right) + M_{wy} \operatorname{cos}\left(\beta\right) - M_{wz} \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right)\right), \quad \dot{R} : \frac{1}{J_{x} J_{z} - J_{xz}^{2}} \left(J_{x} \left(J_{x} P Q + J_{xz} Q R - J_{y} P Q + M_{wx} \operatorname{sin}\left(\alpha\right) + M_{wz} \operatorname{cos}\left(\alpha\right)\right) + J_{xz} \left(J_{xz} P Q - J_{y} Q R + J_{z} Q R - M_{wx} \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) + M_{wy} \operatorname{sin}\left(\beta\right) + M_{wz} \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\right)\right \}$$

In [7]:

    

eoms = {}
eoms.update(sol_euler_rates)
eoms.update(sol_trans)
eoms.update(sol_vel_wind)
eoms.update(sol_rot)

x_vect = sympy.Matrix([Q, theta, V_T, alpha, P, R, phi, beta])
u_vect = sympy.Matrix([elv, thr, ail, rdr])
sub_no_t = {xi(t): xi for xi in x_vect}
f_vect = sympy.Matrix([eoms[xi(t).diff(t)].subs(sub_no_t) for xi in x_vect])
f_vect


    

    
Out[7]:





$$\left[\begin{matrix}\frac{1}{J_{y}} \left(- J_{x} P R + J_{xz} P^{2} - J_{xz} R^{2} + J_{z} P R + M_{wx} \operatorname{sin}\left(\beta\right) \operatorname{cos}\left(\alpha\right) + M_{wy} \operatorname{cos}\left(\beta\right) - M_{wz} \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right)\right)\\Q \operatorname{cos}\left(\phi\right) - R \operatorname{sin}\left(\phi\right)\\g \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\theta\right) + g \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\theta\right) - g \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) - \frac{D}{m} + \frac{T \operatorname{cos}\left(\alpha\right)}{m} \operatorname{cos}\left(\beta\right)\\\frac{1}{V_{T} m} \left(- P V_{T} m \operatorname{sin}\left(\beta\right) + Q V_{T} m \operatorname{cos}\left(\beta\right) - g m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\theta\right) + g m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\beta\right) + g m \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\theta\right) - L - T \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\\- \frac{1}{J_{x} J_{z} - J_{xz}^{2}} \left(J_{xz} \left(J_{x} P Q + J_{xz} Q R - J_{y} P Q + M_{wx} \operatorname{sin}\left(\alpha\right) + M_{wz} \operatorname{cos}\left(\alpha\right)\right) + J_{z} \left(J_{xz} P Q - J_{y} Q R + J_{z} Q R - M_{wx} \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) + M_{wy} \operatorname{sin}\left(\beta\right) + M_{wz} \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\right)\\\frac{1}{J_{x} J_{z} - J_{xz}^{2}} \left(J_{x} \left(J_{x} P Q + J_{xz} Q R - J_{y} P Q + M_{wx} \operatorname{sin}\left(\alpha\right) + M_{wz} \operatorname{cos}\left(\alpha\right)\right) + J_{xz} \left(J_{xz} P Q - J_{y} Q R + J_{z} Q R - M_{wx} \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) + M_{wy} \operatorname{sin}\left(\beta\right) + M_{wz} \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\right)\\P + Q \operatorname{sin}\left(\phi\right) \operatorname{tan}\left(\theta\right) + R \operatorname{cos}\left(\phi\right) \operatorname{tan}\left(\theta\right)\\\frac{1}{V_{T} m \operatorname{cos}\left(\alpha\right)} \left(P V_{T} m \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right) + Q V_{T} m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right) - R V_{T} m \operatorname{cos}\left(\alpha\right) + g m \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\theta\right) + g m \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\beta\right) \operatorname{cos}\left(\theta\right) - C - T \operatorname{sin}\left(\beta\right)\right)\end{matrix}\right]$$

In [8]:

    

C_L_0, C_L_alpha, k_CD_CL, C_D_0, CL_CD_min, rho, S = \
sympy.symbols('C_L0 C_L_alpha k_CD_CL C_D_0 CL_CD_min, rho, S')
C_L = C_L_0 + C_L_alpha*alpha
C_D = C_D_0 + k_CD_CL*(C_L - CL_CD_min)**2
C_C = beta # todo
C_T = 10
C_l = -P -beta + ail
C_m = -Q -alpha + elv
C_n = -R -beta + rdr
q = rho*V_T**2/2
sub_aero_model = {
    L(t): C_L*q*S,
    D(t): C_D*q*S,
    C(t): C_C*q*S,
    T(t): C_T*thr,
    M_wx(t): C_l*q*S,# todo
    M_wy(t): C_m*q*S,# todo
    M_wz(t): C_n*q*S,# todo
}
sub_aero_model


    

    
Out[8]:





$$\left \{ C : \frac{S \beta}{2} V_{T}^{2} \rho, \quad D : \frac{S \rho}{2} V_{T}^{2} \left(C_{D 0} + k_{CD CL} \left(- CL_{CD min} + C_{L0} + C_{L \alpha} \alpha\right)^{2}\right), \quad L : \frac{S \rho}{2} V_{T}^{2} \left(C_{L0} + C_{L \alpha} \alpha\right), \quad M_{wx} : \frac{S \rho}{2} V_{T}^{2} \left(- P + ail - \beta\right), \quad M_{wy} : \frac{S \rho}{2} V_{T}^{2} \left(- Q - \alpha + elv\right), \quad M_{wz} : \frac{S \rho}{2} V_{T}^{2} \left(- R - \beta + rdr\right), \quad T : 10 thr\right \}$$

In [9]:

    

f_aero_vect = f_vect.subs(sub_aero_model).applyfunc(lambda e:e.simplify())
f_aero_vect


    

    
Out[9]:





$$\left[\begin{matrix}\frac{1}{J_{y}} \left(- J_{x} P R + J_{xz} P^{2} - J_{xz} R^{2} + J_{z} P R - \frac{S \rho}{2} V_{T}^{2} \left(P - ail + \beta\right) \operatorname{sin}\left(\beta\right) \operatorname{cos}\left(\alpha\right) - \frac{S \rho}{2} V_{T}^{2} \left(Q + \alpha - elv\right) \operatorname{cos}\left(\beta\right) + \frac{S \rho}{2} V_{T}^{2} \left(R + \beta - rdr\right) \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right)\right)\\Q \operatorname{cos}\left(\phi\right) - R \operatorname{sin}\left(\phi\right)\\\frac{1}{m} \left(- \frac{S \rho}{2} V_{T}^{2} \left(C_{D 0} + k_{CD CL} \left(- CL_{CD min} + C_{L0} + C_{L \alpha} \alpha\right)^{2}\right) + g m \left(\operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\theta\right) + \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\theta\right) - \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right) + 10 thr \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\\\frac{1}{V_{T} m} \left(- P V_{T} m \operatorname{sin}\left(\beta\right) + Q V_{T} m \operatorname{cos}\left(\beta\right) - \frac{S \rho}{2} V_{T}^{2} \left(C_{L0} + C_{L \alpha} \alpha\right) - g m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\theta\right) + g m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\beta\right) + g m \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\phi\right) \operatorname{cos}\left(\theta\right) - 10 thr \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\\\frac{1}{2 J_{x} J_{z} - 2 J_{xz}^{2}} \left(J_{xz} \left(- 2 J_{x} P Q - 2 J_{xz} Q R + 2 J_{y} P Q + S V_{T}^{2} \rho \left(P - ail + \beta\right) \operatorname{sin}\left(\alpha\right) + S V_{T}^{2} \rho \left(R + \beta - rdr\right) \operatorname{cos}\left(\alpha\right)\right) - J_{z} \left(2 J_{xz} P Q - 2 J_{y} Q R + 2 J_{z} Q R + S V_{T}^{2} \rho \left(P - ail + \beta\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) - S V_{T}^{2} \rho \left(Q + \alpha - elv\right) \operatorname{sin}\left(\beta\right) - S V_{T}^{2} \rho \left(R + \beta - rdr\right) \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\right)\\\frac{1}{2 J_{x} J_{z} - 2 J_{xz}^{2}} \left(- J_{x} \left(- 2 J_{x} P Q - 2 J_{xz} Q R + 2 J_{y} P Q + S V_{T}^{2} \rho \left(P - ail + \beta\right) \operatorname{sin}\left(\alpha\right) + S V_{T}^{2} \rho \left(R + \beta - rdr\right) \operatorname{cos}\left(\alpha\right)\right) + J_{xz} \left(2 J_{xz} P Q - 2 J_{y} Q R + 2 J_{z} Q R + S V_{T}^{2} \rho \left(P - ail + \beta\right) \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\beta\right) - S V_{T}^{2} \rho \left(Q + \alpha - elv\right) \operatorname{sin}\left(\beta\right) - S V_{T}^{2} \rho \left(R + \beta - rdr\right) \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right)\right)\right)\\P + Q \operatorname{sin}\left(\phi\right) \operatorname{tan}\left(\theta\right) + R \operatorname{cos}\left(\phi\right) \operatorname{tan}\left(\theta\right)\\\frac{1}{V_{T} m \operatorname{cos}\left(\alpha\right)} \left(P V_{T} m \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\beta\right) + Q V_{T} m \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\beta\right) - R V_{T} m \operatorname{cos}\left(\alpha\right) - \frac{S \beta}{2} V_{T}^{2} \rho + g m \operatorname{sin}\left(\beta\right) \operatorname{sin}\left(\theta\right) + g m \operatorname{sin}\left(\phi\right) \operatorname{cos}\left(\beta\right) \operatorname{cos}\left(\theta\right) - 10 thr \operatorname{sin}\left(\beta\right)\right)\end{matrix}\right]$$

In [10]:

    

param = f_aero_vect.atoms(sympy.Symbol).difference(x_vect)
param


    

    
Out[10]:





$$\left\{CL_{CD min}, C_{D 0}, C_{L0}, C_{L \alpha}, J_{x}, J_{xz}, J_{y}, J_{z}, S, ail, elv, g, k_{CD CL}, m, rdr, \rho, thr\right\}$$

In [11]:

    

#x_id = sympy.Matrix(list(x_vect) + list(param))
#f_id_vect = sympy.Matrix(list(f_aero_vect) + list(sympy.zeros(len(param))))
#A_id = f_id_vect.jacobian(x_id)
#sub_x0 = {P:0, Q:0, R:0, phi:0, theta:0, alpha:0, beta:0}
#A_id.subs(sub_x0)


    

In [12]:

    

sub_const = {
    J_x:1, J_y:1, J_z:1, J_xz:0, m:1, rho:1.225, S:1, g:10,
    C_L_alpha:3, C_L_0: 0.1, C_D_0: 0.01, k_CD_CL: 0.01, CL_CD_min:0,
}


    

In [13]:

    

x_vect.T, u_vect.T


    

    
Out[13]:





$$\left ( \left[\begin{matrix}Q & \theta & V_{T} & \alpha & P & R & \phi & \beta\end{matrix}\right], \quad \left[\begin{matrix}elv & thr & ail & rdr\end{matrix}\right]\right )$$

In [14]:

    

def trim_level(V_T, gamma):
    f1 = lambda theta, elv, thr: \
        pl.sum(ss.f_eval(0, [0, theta, V_T, gamma-theta, 0, 0, 0, 0], [elv, thr, 0, 0])**2)
    res = scipy.optimize.minimize(lambda x:f1(theta=x[0], elv=x[1], thr=x[2]), [0,0,0])
    theta_trim = res.x[0]
    elv_trim = res.x[1]
    thr_trim = res.x[2]
    return [0, theta_trim, V_T, gamma - theta_trim, 0, 0, 0, 0], [elv_trim, thr_trim, 0, 0], res


    

In [15]:

    

ss = statespace.StateSpace(x_vect, u_vect, f_aero_vect.subs(sub_const), x_vect)
x0, u0, res = trim_level(20, 0)
data = ss.simulate(x0, u0,dt=0.01,tf=60)
pl.plot(data.t, data.y);
pl.grid()
x0, u0


    

    
Out[15]:





$$\left ( \left [ 0, \quad 0.0196622211004, \quad 20, \quad -0.0196622211004, \quad 0, \quad 0, \quad 0, \quad 0\right ], \quad \left [ -0.0196622285512, \quad 0.284781460076, \quad 0, \quad 0\right ]\right )$$






    

In [16]:

    

ss_lin = ss.linearize(x0, u0)
control.bode(ss_lin.to_control()[0,0], omega=pl.logspace(-2,2));


    

    




/usr/local/lib/python2.7/dist-packages/control/freqplot.py:124: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
  if (omega == None):






    

In [17]:

    

control.rlocus(ss_lin.to_control()[0,0], klist=pl.linspace(0,1,1000));
pl.grid()


    

    




/usr/local/lib/python2.7/dist-packages/control/matlab.py:1136: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
  if klist == None:






    

In [18]:

    

C_L_eval = sympy.lambdify(alpha, C_L.subs(sub_const))
C_D_eval = sympy.lambdify(alpha, C_D.subs(sub_const))
alpha_val = pl.linspace(-pl.deg2rad(15),pl.deg2rad(15))

pl.figure()
pl.plot(alpha_val, C_L_eval(alpha_val))
pl.xlabel(r'$\alpha$')
pl.ylabel(r'$C_L$')
pl.grid()

pl.figure()
pl.plot(C_L_eval(alpha_val), C_D_eval(alpha_val))
pl.xlabel(r'$C_L$')
pl.ylabel(r'$C_D$')
pl.grid()


    

In [19]:

    

K, S, E = control.lqr(ss_lin.A, ss_lin.B, pl.eye(8), pl.eye(4))
K


    

    
Out[19]:





array([[  4.175e-01,   1.411e+00,  -4.802e-02,  -2.839e-01,   2.836e-14,
          3.277e-13,  -9.850e-13,   2.515e-13],
       [ -1.971e-03,  -9.046e-01,   9.736e-01,   2.035e-01,  -2.513e-15,
         -1.947e-14,   1.887e-12,  -1.215e-12],
       [  2.191e-14,  -7.459e-13,  -5.215e-14,  -3.617e-14,   4.170e-01,
         -7.601e-03,   9.713e-01,  -3.250e-01],
       [  3.282e-13,  -2.508e-12,  -4.783e-13,  -7.556e-15,   8.763e-03,
          4.152e-01,   6.116e-02,  -3.387e-01]])

In [20]:

    

class Autopilot(object):
    """
    This class is a simple autopilot with an estimator and controller.
    """
    def __init__(self, x0, u0, f_eval, g_eval):
        self.time_contr = 0
        self.time_est = 0
        self.ode = scipy.integrate.ode(f_eval)

    def predict(self, t):
        self.ode.integrate(t)


    

In [21]:

    

def f_contr(t, y):
    x = y
    return u0-K.dot(x-x0)


    

In [22]:

    

x1 = x0 + pl.rand(8)
data_lin = ss_lin.simulate(x1, u0, contr_eval=f_contr, dt=0.02)
data = ss.simulate(x1, u0, contr_eval=f_contr, dt=0.02)
h0 = pl.plot(data.t, data.x, 'b');
h1 = pl.plot(data_lin.t, data_lin.x, 'r');
pl.legend([h0[0], h1[0]], ['non-lin', 'lin'], loc='best')
pl.grid()


    

In [23]:

    

pl.plot(data.t, data_lin.x - data.x);
pl.title('linearization error')
pl.grid()


    

In [24]:

    

pl.plot(data.t, data.u);
pl.grid()


    

In [25]:

    

K_clean = pl.where(abs(K) < 1e-1, pl.zeros(K.shape), K)
K_clean


    

    
Out[25]:





array([[ 0.417,  1.411,  0.   , -0.284,  0.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   , -0.905,  0.974,  0.203,  0.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  0.   ,  0.417,  0.   ,  0.971, -0.325],
       [ 0.   ,  0.   ,  0.   ,  0.   ,  0.   ,  0.415,  0.   , -0.339]])

In [26]:

    

P_r, Q_r, R_r, k_P, k_Q, k_R, k_theta, k_phi, k_alpha, k_dh, k_ah, dh_r, V_T_r, k_V_thr, k_rdr_beta, k_ail_beta, k_alpha_thr, alpha_r = \
    sympy.symbols('P_r, Q_r, R_r, k_P, k_Q, k_R, k_theta, k_phi, k_alpha, k_dh, k_ah, dh_r, V_T_r, k_V_thr, k_rdr_beta, k_ail_beta, k_alpha_thr, alpha_r')


    

In [27]:

    

phi_r = k_P *(P_r - P)
ail_r = k_phi*(phi_r - phi) + k_ail_beta*beta

theta_r = k_Q *(Q_r - Q)

elv_r = k_theta*(theta_r - theta) - k_alpha*(alpha_r - alpha)
rdr_r = k_R *(R_r - R) + k_rdr_beta*beta

thr_r = -k_dh*(dh_r - V_T*sympy.sin(theta - alpha)) + k_V_thr*(V_T_r - V_T) - k_alpha_thr*(alpha_r - alpha)
u_r = sympy.Matrix([elv_r, thr_r, ail_r, rdr_r])
u_r


    

    
Out[27]:





$$\left[\begin{matrix}- k_{\alpha} \left(- \alpha + \alpha_{r}\right) + k_{\theta} \left(k_{Q} \left(- Q + Q_{r}\right) - \theta\right)\\k_{V thr} \left(- V_{T} + V_{T r}\right) - k_{\alpha thr} \left(- \alpha + \alpha_{r}\right) - k_{dh} \left(V_{T} \operatorname{sin}\left(\alpha - \theta\right) + dh_{r}\right)\\\beta k_{ail \beta} + k_{\phi} \left(k_{P} \left(- P + P_{r}\right) - \phi\right)\\\beta k_{rdr \beta} + k_{R} \left(- R + R_{r}\right)\end{matrix}\right]$$

In [28]:

    

u_vect.T, x_vect.T


    

    
Out[28]:





$$\left ( \left[\begin{matrix}elv & thr & ail & rdr\end{matrix}\right], \quad \left[\begin{matrix}Q & \theta & V_{T} & \alpha & P & R & \phi & \beta\end{matrix}\right]\right )$$

In [29]:

    

sub_trim = {x_vect[i]: x0[i] for i in range(len(x_vect))}
sub_trim.update({u_vect[i]: u0[i] for i in range(len(u_vect))})
sub_trim


    

    
Out[29]:





$$\left \{ P : 0, \quad Q : 0, \quad R : 0, \quad V_{T} : 20, \quad ail : 0, \quad \alpha : -0.0196622211004, \quad \beta : 0, \quad elv : -0.0196622285512, \quad \phi : 0, \quad rdr : 0, \quad \theta : 0.0196622211004, \quad thr : 0.284781460076\right \}$$

In [30]:

    

u_r = sympy.Matrix([elv_r, thr_r, ail_r, rdr_r])
K_syms = -u_r.jacobian(x_vect).subs(sub_trim)
K_syms


    

    
Out[30]:





$$\left[\begin{matrix}k_{Q} k_{\theta} & k_{\theta} & 0 & - k_{\alpha} & 0 & 0 & 0 & 0\\0 & - 19.9845378751776 k_{dh} & k_{V thr} - 0.039314307687877 k_{dh} & - k_{\alpha thr} + 19.9845378751776 k_{dh} & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & k_{P} k_{\phi} & 0 & k_{\phi} & - k_{ail \beta}\\0 & 0 & 0 & 0 & 0 & k_{R} & 0 & - k_{rdr \beta}\end{matrix}\right]$$

In [31]:

    

K_diff = list(pl.where(abs(K) < 1e-1, pl.zeros(K.shape), K) - K_syms)
K_eqs = sympy.Matrix([ K_diff[i] for i in pl.nonzero(K_diff)[0]])
K_eqs


    

    
Out[31]:





$$\left[\begin{matrix}- k_{Q} k_{\theta} + 0.417461280566769\\- k_{\theta} + 1.4109761152722\\k_{\alpha} - 0.283931106866936\\19.9845378751776 k_{dh} - 0.904595697355912\\- k_{V thr} + 0.039314307687877 k_{dh} + 0.973583929069631\\k_{\alpha thr} - 19.9845378751776 k_{dh} + 0.203499495423821\\- k_{P} k_{\phi} + 0.417008809476205\\- k_{\phi} + 0.971332589512055\\k_{ail \beta} - 0.324971897058001\\- k_{R} + 0.415154150886853\\k_{rdr \beta} - 0.338711658534966\end{matrix}\right]$$

In [32]:

    

len(K_eqs), len(K_eqs.atoms(sympy.Symbol))


    

    
Out[32]:





$$\left ( 11, \quad 11\right )$$

In [33]:

    

sol_K = sympy.solve(K_eqs)[0]
sol_K


    

    
Out[33]:





$$\left \{ k_{P} : 0.429316193010355, \quad k_{Q} : 0.295867007278315, \quad k_{R} : 0.415154150886853, \quad k_{V thr} : 0.975363482532469, \quad k_{ail \beta} : 0.324971897058001, \quad k_{\alpha} : 0.283931106866936, \quad k_{\alpha thr} : 0.701096201932091, \quad k_{dh} : 0.0452647793512149, \quad k_{\phi} : 0.971332589512055, \quad k_{rdr \beta} : 0.338711658534966, \quad k_{\theta} : 1.4109761152722\right \}$$

In [34]:

    

K_d = pl.array(K_syms.subs(sol_K)).astype(float)
K_d


    

    
Out[34]:





array([[ 0.417,  1.411,  0.   , -0.284,  0.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   , -0.905,  0.974,  0.203,  0.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  0.   ,  0.417,  0.   ,  0.971, -0.325],
       [ 0.   ,  0.   ,  0.   ,  0.   ,  0.   ,  0.415,  0.   , -0.339]])

In [35]:

    

K_clean


    

    
Out[35]:





array([[ 0.417,  1.411,  0.   , -0.284,  0.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   , -0.905,  0.974,  0.203,  0.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  0.   ,  0.417,  0.   ,  0.971, -0.325],
       [ 0.   ,  0.   ,  0.   ,  0.   ,  0.   ,  0.415,  0.   , -0.339]])

In [36]:

    

pl.eig(ss_lin.A - ss_lin.B.dot(K_d))[0]


    

    
Out[36]:





array([-345.710+0.j   ,  -37.442+0.j   ,   -0.984+0.j   ,   -9.995+0.j   ,
       -346.777+7.043j, -346.777-7.043j,   -0.709+0.j   ,  -11.868+0.j   ])

In [37]:

    

pl.eig(ss_lin.A - ss_lin.B.dot(K_clean))[0]


    

    
Out[37]:





array([-345.710+0.j   ,  -37.442+0.j   ,   -0.984+0.j   ,   -9.995+0.j   ,
       -346.777+7.043j, -346.777-7.043j,   -0.709+0.j   ,  -11.868+0.j   ])

In [38]:

    

pl.eig(ss_lin.A - ss_lin.B.dot(K))[0]


    

    
Out[38]:





array([-345.710+0.j   , -346.814+5.048j, -346.814-5.048j,  -37.453+0.j   ,
         -0.989+0.j   ,   -9.980+0.j   ,   -0.712+0.j   ,  -11.869+0.j   ])